package legoserver;

import lejos.pc.comm.*;
import java.io.*;
import java.awt.*;
import java.util.*;

/**
 * This class manages and connects to the robot using a bluetooth connection
 * The rest of the server is incomplete, and totally interoperable with the
 * working robot code, so this code will have to be run as an independant
 * process for now.  Later this should be implemented and managed as a
 * thread off of the main server program.  That means syncronization, and
 * communication. (Ie. server receives a command from the client and processes
 * it, but then it has to talk to the robot connection thread to communicate
 * with the robot).
 * 
 * @author Daniel Van Eck
 */
public class RobotConnection extends Thread {

private final String name = "LOUIE";
private final String address = "00:16:53:06:54:73"; // US Robot
//private final String address = "00:16:53:02:c4:f1";   // SE Robot
//Robot response strings
private final String okString      = "OK";
private final String readyString   = "ReadyForCmd";

public static final String MOVE_FORWARD  = "MoveForward";
public static final String MOVE_BACKWARD = "MoveBackward";
public static final String TURN_LEFT     = "TurnLeft";
public static final String TURN_RIGHT    = "TurnRight";
public static final String STOP          = "Stop";
    
//Bluetooth connection
private NXTComm btConn;

//Input and output data streams
private DataInputStream bt_dInStream;
private DataOutputStream bt_dOutStream;

//Robot information data structure
private NXTInfo[] nxtInfo;

//set to true if the thread is busy with some task, such as communicating with
//the robot, that would prevent it from receiving another command
//Starts off true (since the thread will be connecting to the robot at first
//TODO I don't honestly know if this is needed anyways... we'll see
private boolean busy;

//Gui for message displaying
private ServerGUI gui;

//whether we are currently connected to the robot or not
private boolean connected;

//try to stop navigating if this is false
private boolean stopping;

//Direction constants
public static final int NORTH = 0;
public static final int SOUTH = 1;
public static final int EAST = 2;
public static final int WEST = 3;
private int facing = NORTH; //default pointing is north

public RobotConnection(ServerGUI gui) {
    connected = false;
    busy = true;
    stopping = false;
    this.gui = gui;
}

public void run() {
    try {
        while(!connect())
        {
            connected = false;
            //wait and try again
            try {
                Thread.sleep(500);
            }catch(InterruptedException e) {
                //let it slide and keep waiting
            }
        }
    }catch(ExceptionInInitializerError e) {
        update("Bluetooth not detected.");
        update("For testing purposes, staying alive...");
        connected = true;
    }
    
    while(connected) {
        //keep thread alive, checking every 100 ms
        try {
            Thread.sleep(1000);
        }catch(InterruptedException e) {
            //keep on waiting.
        }
        if(!connected) {
            //try to reconnect every bit
            while(!connect()) {
                connected = false;
                try {
                    Thread.sleep(500);
                }catch(InterruptedException e) {
                    //let it slide and keep waiting
                }
            }
        }
    }
}

/* 
 * Send a cmd over the BT connection
 */
public boolean sendCmd(String sCmd) throws IOException, NullPointerException {
    //thread is now busy, but doesn't matter now since it's not a thread yet
    busy = true;
    
    if(sCmd.equals(STOP)) {
        stopping = true;
        return true;
    }
    
    //Every command has to go through the basic handshake
    try{
        if(!readString().equals(readyString))
            update("Bad thing happened (incorrect string received from robot");
    }catch(IOException e) {
        busy = false;
        disconnect();
        return false;
    }

     
    
    if( sCmd.equals(MOVE_FORWARD) || sCmd.equals(MOVE_BACKWARD) ||
        sCmd.equals(TURN_LEFT)    || sCmd.equals(TURN_RIGHT)    ||
        sCmd.equals("shutdown")   || sCmd.equals("backtrack")   ||
        sCmd.equals("disconnecting")){
        
        //Send the cmd
        try{
            writeString(sCmd);
        }
        catch(IOException e){
            update("Failed to send sCmd to robot.");
            busy = false;
            disconnect();
            return false;
        }
        
        if (sCmd.equals(TURN_LEFT)) {
            //update facing
            switch(facing){
                case NORTH:
                    facing = WEST;
                case SOUTH:
                    facing = EAST;
                case EAST:
                    facing = NORTH;
                case WEST:
                    facing = SOUTH;
            }
        }
        
        if (sCmd.equals(TURN_RIGHT)) {
            //update facing
            switch(facing){
                case NORTH:
                    facing = EAST;
                case SOUTH:
                    facing = WEST;
                case EAST:
                    facing = SOUTH;
                case WEST:
                    facing = NORTH;
            }
        }

        try{
            if(!readString().equals(okString))
                update("Incorrect string received from robot.");
        }catch(IOException e) {
            busy = false;
            disconnect();
            return false;
        }
        
        //try to close the bt streams
        if(sCmd.equals("shutdown")) {
            if(bt_dInStream != null)
                bt_dInStream.close();

            if(bt_dOutStream != null)
                bt_dOutStream.close();

            if(btConn != null)
                btConn.close();

            connected = false;
        }
    }
    else {
        update("Invalid Command...");
        busy = false;
        disconnect();
        return false;
    }
    
    busy = false;
    return true;
}

/*
 * Also System.out statements will hopefully be put into a GUI at some point,
 * as a default console output for the server is kind of tacky.
 * 
 */
public boolean connect() throws ExceptionInInitializerError {
    busy = true;
    btConn = NXTCommFactory.createNXTComm(NXTCommFactory.BLUETOOTH);
    
    nxtInfo = new NXTInfo[1];
    nxtInfo[0] = new NXTInfo(name,address);
    
    update("Connecting to " + nxtInfo[0].btResourceString);
    
    try {
        if(!(btConn.open(nxtInfo[0]))) {
            update("Failed to open " + nxtInfo[0].name);
            return false;
        }
    } catch (NXTCommException e) {
        update("Exception opening connection!");
        return false;
    }
    
    update("Connected to " + nxtInfo[0].btResourceString);
    
    bt_dInStream = new DataInputStream(btConn.getInputStream());
    bt_dOutStream = new DataOutputStream(btConn.getOutputStream());
    
    connected = true;
    busy = false;
    return true;
}

public void disconnect() throws IOException, NullPointerException {
    busy = true;
    
    /*if(!sendCmd("shutdown"))
        update("Send shutdown command to robot failed.");*/
    //do this instead.  maybe we can add a command to shutdown the robot, but
    //for now we only need this
    if(!sendCmd("disconnecting"))
        update("Notify robot of disconnect failed.");
    
    if(bt_dInStream != null)
        bt_dInStream.close();
    
    if(bt_dOutStream != null)
        bt_dOutStream.close();
    
    if(btConn != null)
        btConn.close();
    
    connected = false;
    busy = false;
}

public void navigateToPoint(Vector solution) {
    busy = true;
    int size = solution.size();
    Point[] solutionEdges = new Point[size];
            
    for(int i = 0; i < size; i++) {
        solutionEdges[i] = (Point)solution.elementAt(i);
    }
    
    try {
        Point curPos = solutionEdges[0];

        for(int i = 1; i < size; i++) {
            if(stopping) {
                update("Stopping the robot.");
                busy = false;
                return;
            }
                
            
            //Go west one
            while(curPos.x > solutionEdges[i].x) {
                moveWestOne();
                curPos.x--;
            }//end-while
            while(curPos.x < solutionEdges[i].x) {
                moveEastOne();
                curPos.x++;
            }//end-while
            while(curPos.y > solutionEdges[i].y) {
                moveSouthOne();
                curPos.y--;
            }//end-while
            while(curPos.y < solutionEdges[i].y) {
                moveNorthOne();
                curPos.y++;
            }//end-while
        }//end-for
    }catch(IOException e) {
        update("IOException during navigation.");
        update("Returning to start.");
        try { // try again to tell the robot to go back to start
            sendCmd("backtrack");
        }catch(IOException newE) {
            update("Couldn't tell robot to return to start.");
        }//end-catch
    }//end-catch
    
    busy = false;
}//end-navigateToPoint()

/**
 * Used by navigateToPoint()
 * 
 * @param facing Current facing direction
 * @return New facing
 */
private void moveEastOne() throws IOException {
    switch(facing) {
        case NORTH:
            sendCmd(TURN_RIGHT);
            sendCmd(MOVE_FORWARD);
            facing = EAST;
            break;
            
        case WEST:
            sendCmd(MOVE_BACKWARD);
            break;
            
        case EAST:
            sendCmd(MOVE_FORWARD);
            break;
            
        case SOUTH:
            sendCmd(TURN_LEFT);
            sendCmd(MOVE_FORWARD);
            facing = EAST;
            break;
            
        default:
            update("Variable direction facing is invalid.");
            return;
    }//end-switch
}//end-moveEastOne()

/**
 * Used by navigateToPoint()
 * 
 * @param facing Current facing direction
 * @return New facing
 */
private void moveWestOne() throws IOException {
    switch(facing) {
        case NORTH:
            sendCmd(TURN_LEFT);
            sendCmd(MOVE_FORWARD);
            facing = WEST;
            break;
            
        case WEST:
            sendCmd(MOVE_FORWARD);
            break;
            
        case EAST:
            sendCmd(MOVE_BACKWARD);
            break;
            
        case SOUTH:
            sendCmd(TURN_RIGHT);
            sendCmd(MOVE_FORWARD);
            facing = WEST;
            break;
            
        default:
            update("Variable direction facing is invalid.");
            return;
    }//end-switch
}

/**
 * Used by navigateToPoint()
 * 
 * @param facing Current facing direction
 * @return New facing
 */
private void moveSouthOne() throws IOException {
    switch(facing) {
        case NORTH:
            sendCmd(MOVE_BACKWARD);
            break;
            
        case WEST:
            sendCmd(TURN_LEFT);
            sendCmd(MOVE_FORWARD);
            facing = SOUTH;
            break;
            
        case EAST:
            sendCmd(TURN_RIGHT);
            sendCmd(MOVE_FORWARD);
            facing = SOUTH;
            break;
            
        case SOUTH:
            sendCmd(MOVE_FORWARD);
            break;
            
        default:
            update("Variable direction facing is invalid.");
            return;
    }//end-switch
}

/**
 * Used by navigateToPoint()
 * 
 * @param facing Current facing direction
 * @return New facing
 */
private void moveNorthOne() throws IOException {
    switch(facing) {
        case NORTH:
            sendCmd(MOVE_FORWARD);
            break;
            
        case WEST:
            sendCmd(TURN_RIGHT);
            sendCmd(MOVE_FORWARD);
            facing = NORTH;
            break;
            
        case EAST:
            sendCmd(TURN_LEFT);
            sendCmd(MOVE_FORWARD);
            facing = NORTH;
            break;
            
        case SOUTH:
            sendCmd(MOVE_BACKWARD);
            break;
            
        default:
            update("Variable direction facing is invalid.");
            return;
    }//end-switch
}

/*
 * Send a string to the robotb, one character at a time
 */
public boolean writeString(String s) throws IOException{

    if(s.length() < 1 || s == null)
        return false;

    for(int i = 0; i < s.length(); i++) {
        bt_dOutStream.writeChar(s.charAt(i));
        bt_dOutStream.flush();
    }
    bt_dOutStream.writeChar('*');
    bt_dOutStream.flush();

    return true;
}

/*
 * Read a string from the robot, one character at a time
 */
public String readString() throws IOException {
    char c;
    String temp = "";

    while((c = bt_dInStream.readChar()) != '*')
        temp += c;

    return temp;
}

public boolean isBusy() {
    return busy;
}

public boolean isConnected() {
    return connected;
}

public void update(String s) {
    if(gui != null)
        gui.update("RobotConn: " + s);
}

// All of this code is to test drive the server... it will be totally
// unnecessary once some sort of client is implemented
/*public static void main(String args[]) {
    RobotConnection rConn = new RobotConnection();
    
    if(!rConn.connect())
        System.exit(1);
    
    // temporary reader for command inputs
    BufferedReader brIn = new BufferedReader(new InputStreamReader(System.in));
    
    String rawCmd;
    int cmd = 0;
    
    
    while(cmd != 5 && rConn.connected) {
        // enter an integer command
        try {
            printMenuItems();
            rawCmd = brIn.readLine();
            cmd = Integer.parseInt(rawCmd);
        } catch (IOException e) {
            e.printStackTrace();
            rConn.disconnect();
            System.exit(1);
            return;  //so compiler won't complain about cmd being un-initialized
        }

        System.out.print("Sending command: ");
        switch (cmd) {
            case 1:
                System.out.println("Move Forward...");
                rConn.sendCmd(rConn.MOVE_FORWARD);
                break;
            case 2:
                System.out.println("Move Backward...");
                rConn.sendCmd(rConn.MOVE_BACKWARD);
                break;
            case 3:
                System.out.println("Turn left...");
                rConn.sendCmd(rConn.TURN_LEFT);
                break;
            case 4:
                System.out.println("Turn right...");
                rConn.sendCmd(rConn.TURN_RIGHT);
                break;
            case 5:
                System.out.println();
                System.out.println("Exiting...");
                rConn.sendCmd("shutdown");
                break;
            default:
                System.out.println();
                System.out.println("Invalid command!");
        }
    }
    
}

//print a simple menu to console... this'll have to do until client/server
//integration is achieved
public static void printMenuItems() {
    System.out.println("Different available commands:");
    System.out.println("(1) Move forward");
    System.out.println("(2) Move backward");
    System.out.println("(3) Turn left");
    System.out.println("(4) Turn right");
    System.out.println("(5) Exit");
    System.out.print(  "Enter choice: ");
}*/
}//end-RobotConnection
